#include "brush.h"
#include "../common/common_motor.h"
#include "../../bsp/xp_log.h"
#include "../../bsp/bsp.h"
#include "zbld_c20.h"
#include <stdlib.h>

#define BRUSH_REDUCER_RATIO     (30)

#define BRUSH_ERROR(format, args...)    xlog_error("<brush>"format, ##args)
#define BRUSH_WARN(format, args...)     xlog_warn("<brush>"format, ##args)
#define BRUSH_INFO(format, args...)     xlog_info("<brush>"format, ##args)
#define BRUSH_DEBUG(format, args...)    xlog_debug("<brush>"format, ##args)

struct {
    short error_code;
    short current;
    struct xp_motor motor;
} brush;

struct zbld_motor brush_motor_dev = {
    .bus_port = 2,
    .baudrate = 19200,
    .sid = 1,
    .dir = motor_forward,
    .run = 0,
    .rpm = 0,
};

static int brush_debug(char *type, char *fun, char *param);
// 刷子初始化
xp_bool brush_init()
{
    int ret = xp_modbus_init(brush_motor_dev.bus_port, brush_motor_dev.baudrate, MB_PAR_NONE, 100);
    if (ret != 0) return xp_false;
    // 构造电机
    zbld_motor_constructor(&brush.motor, &brush_motor_dev, (uint8_t *)"brush_motor");
    // 电机初始化
    brush.motor.ops->init(&brush.motor);
    brush.motor.inited = 1;
    xp_cli_debug_add(brush_debug);
    return xp_true;
}

#define LOCAL_ASSERT()  if (brush.motor.inited != 1) return xp_false;

// 刷子转动 设置速度 速度正负表示方向 0表示停止转动
xp_bool brush_rotate(short rpm)
{
    LOCAL_ASSERT();
    // rpm*减速比
    motor_direction_t dir = (rpm < 0) ? motor_reverser : motor_forward;
    short rpm_abs = (rpm < 0) ? 0 - rpm : rpm;
    short motor_rpm = rpm_abs * BRUSH_REDUCER_RATIO;

    if (motor_rpm == 0) {
        if (0 != brush.motor.ops->rotation_stop(&brush.motor)) {
            brush.motor.ops->emerg_enable(&brush.motor);
        }
        BRUSH_INFO("updated brush speed to %drpm.", rpm);
        return xp_true;
    }

    int ret = brush.motor.ops->speed_set(&brush.motor, motor_rpm);
    if (ret != 0) {
        BRUSH_ERROR("set brush motor speed failed.");
        return xp_false;
    }
    ret = brush.motor.ops->direction_set(&brush.motor, dir);
    if (ret != 0) {
        BRUSH_ERROR("set brush motor direction failed.");
        return xp_false;
    }
    ret = brush.motor.ops->rotation_start(&brush.motor);
    if (ret != 0) {
        BRUSH_ERROR("set brush motor start failed.");
        return xp_false;
    }

    BRUSH_INFO("updated brush speed to %drpm.", rpm);
    return xp_true;
}

// 检查刷子io是否报警
xp_bool brush_has_warn()
{
    LOCAL_ASSERT();
    // 读IO判断
    if (0 /* has some warn */ ) {
        brush.motor.ops->error_get(&brush.motor, &brush.error_code);
        BRUSH_WARN("brush motor report warning {\"error_code\" : %d}", brush.error_code);
        return xp_true;
    }
    return xp_false;
}

// 读取刷子电机输出电流值
xp_bool brush_current_get(short *current)
{
    LOCAL_ASSERT();
    union {
        short sh;
        char ch[2];
    } local_current;
    int ret = zbld_current_get(&brush.motor, &local_current.sh);
    if (ret != 0) {
        BRUSH_WARN("brush current read failed.");
        return xp_false;
    }
    *current = local_current.ch[0];
    *current <<= 8;
    *current += local_current.ch[1];
    BRUSH_DEBUG("brush current read OK{\"current\" : %d}.", *current);
    return xp_true;
}

// 清除刷子电机报警
xp_bool brush_warn_clear()
{
    LOCAL_ASSERT();
    brush.motor.ops->error_clear(&brush.motor);
}

// 刷子紧急停止
xp_bool brush_emerg_stop()
{
    LOCAL_ASSERT();
    brush.motor.ops->emerg_enable(&brush.motor);
}

// debug
static int brush_debug(char *type, char *fun, char *param)
{
    if (0 != strcmp(type, "brush")) return 0;
    if (0 == strcmp(fun, "rotate")) {
        brush_rotate(atoi(param));
    } else if (0 == strcmp(fun, "current_get")) {
        short current = 0;
        brush_current_get(&current);
        BRUSH_DEBUG("CURRENT : %d", current);
    }
    
    return 1;
}

